#include <ros/ros.h>
#include "controller/dji_controller.h"
#include <dji_osdk_ros/FlightTaskControl.h>
#include <dji_osdk_ros/ObtainControlAuthority.h>
#include "quad_msgs/Trigger.h"

using namespace dji_osdk_ros;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "dji_controller");
    ros::NodeHandle nh;

    // 初始化无人机当前位姿
    Planner::DJIController dji_controller(nh);
    ros::Rate rate(1);
    for (int i = 0; i < 5; i++)
    {
        ros::spinOnce();
        rate.sleep();
    }

    auto obtain_ctrl_authority_client = nh.serviceClient<dji_osdk_ros::ObtainControlAuthority>("obtain_release_control_authority");
    ObtainControlAuthority obtainCtrlAuthority;
    obtainCtrlAuthority.request.enable_obtain = true;
    obtain_ctrl_authority_client.call(obtainCtrlAuthority);

    ros::ServiceClient task_control_client = nh.serviceClient<FlightTaskControl>("/flight_task_control");
    FlightTaskControl control_task;
    control_task.request.task = FlightTaskControl::Request::TASK_TAKEOFF;
    ROS_INFO_STREAM("Takeoff request sending ...");
    task_control_client.call(control_task);
    if (control_task.response.result == false)
    {
        ROS_ERROR_STREAM("Takeoff task failed");
        return 0;
    }
    ROS_INFO_STREAM("Takeoff task successful");
    ros::Duration(2.0).sleep();

    // 发送初始化请求至traj_server
    ros::ServiceClient trigger_client_ = nh.serviceClient<quad_msgs::Trigger>("trigger_traj_server");
    quad_msgs::Trigger trigger_srv;
    trigger_srv.request.trigger = true;
    if (trigger_client_.call(trigger_srv) && trigger_srv.response.ack)
    {
        ROS_INFO("Traj_server triggered successfully");
    }
    else
    {
        ROS_ERROR("Cannot trigger traj_server");
        // TODO 无人机降落

        return 0;
    }

    ros::spin();
    return 0;
}
